﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using BEPUphysics.Constraints;
using BEPUphysics.Entities;
using BEPUphysics.UpdateableSystems;
using BEPUphysics.Constraints.SingleEntity;
using BEPUphysics.Constraints.TwoEntity.Motors;
using BEPUphysics;

namespace NOVA.Physics
{
    /// <summary>
    /// Pushes an entity around according to goal positions and orientations.
    /// </summary>
    public class EntityTransformer : Updateable, IEndOfFrameUpdateable//IDuringForcesUpdateable
    {
        /// <summary>
        /// Gets the angular motor used by the entity mover.
        /// When the affected entity is dynamic, it is pushed by motors.
        /// This ensures that its interactions and collisions with
        /// other entities remain stable.
        /// </summary>
        public SingleEntityAngularMotor AngularMotor { get; private set; }
        /// <summary>
        /// Gets the linear motor used by the entity mover.
        /// When the affected entity is dynamic, it is pushed by motors.
        /// This ensures that its interactions and collisions with
        /// other entities remain stable.
        /// </summary>
        public SingleEntityLinearMotor LinearMotor { get; private set; }

        /// <summary>
        /// Gets or sets the target location of the entity mover.
        /// </summary>
        public Vector3 TargetPosition { get; set; }
        /// <summary>
        /// Gets or sets the target orientation of the entity mover.
        /// </summary>
        public Quaternion TargetOrientation { get; set; }

        Entity entity;
        public bool m_bEnabled;

        /// <summary>
        /// Gets or sets the entity being pushed by the entity mover.
        /// </summary>
        public Entity Entity
        {
            get { return entity; }
            set
            {
                entity = value;
                AngularMotor.Entity = value;
                LinearMotor.Entity = value;

                if (value != null)
                {
                    Setup(value);
                    LinearMotor.IsActive = true;
                    AngularMotor.IsActive = true;
                    IsUpdating = true;
                }
                else
                {
                    LinearMotor.IsActive = false;
                    AngularMotor.IsActive = false;
                    IsUpdating = false;
                }
            }
        }

        /// <summary>
        /// Constructs a new EntityMover.
        /// </summary>
        public EntityTransformer()
        {
            LinearMotor = new SingleEntityLinearMotor();
            AngularMotor = new SingleEntityAngularMotor();

            LinearMotor.Settings.Mode = MotorMode.Servomechanism;
            AngularMotor.Settings.Mode = MotorMode.Servomechanism;

            m_bEnabled = false;
            IsUpdating = false;
        }

        /// <summary>
        /// Initiates a new EntityMover.
        /// </summary>
        /// <param name="e">Entity to move.</param>
        public void Setup(Entity e)
        {
            IsUpdatedSequentially = false;
            //this.Entity = e;
            /*
            LinearMotor.Settings.Servo.SpringSettings.StiffnessConstant = 60000 * e.Mass;
            LinearMotor.Settings.Servo.SpringSettings.DampingConstant = 10000 * e.Mass;

            //angularMotor.Settings.VelocityMotor.Softness = .1f / e.Mass;
            AngularMotor.Settings.Servo.SpringSettings.StiffnessConstant = 60000 * e.Mass;
            AngularMotor.Settings.Servo.SpringSettings.DampingConstant = 10000 * e.Mass;

            //An unlimited motor will gladly push the entity through other objects.
            //Putting a limit on the strength of the motor will prevent it from doing so.
            LinearMotor.Settings.MaximumForce = 10 * e.Mass;
            AngularMotor.Settings.MaximumForce = 100 * e.Mass;
            */
            TargetPosition = e.Position;
            TargetOrientation = e.Orientation;

            LinearMotor.Settings.Servo.Goal = e.Position;
            AngularMotor.Settings.Servo.Goal = e.Orientation;

            m_bEnabled = true;
        }

        /// <summary>
        /// Called automatically by the space.
        /// </summary>
        /// <param name="dt">Simulation timestep.</param>
        void IEndOfFrameUpdateable.Update(float dt)
        {
            if (!m_bEnabled)
            {
                if (entity != LinearMotor.Entity || entity != AngularMotor.Entity)
                    throw new InvalidOperationException("EntityMover's entity differs from EntityMover's motors' entities.  Ensure that the moved entity is only changed by setting the EntityMover's entity property.");
                if (entity.Space != Space)
                    throw new InvalidOperationException("EntityMover's entity must belong to the EntityMover's space.  Ensure the entity has been added to the space.");
                if (entity.IsDynamic)
                {
                    LinearMotor.IsActive = true;
                    AngularMotor.IsActive = true;
                    LinearMotor.Settings.Servo.Goal = TargetPosition;
                    AngularMotor.Settings.Servo.Goal = TargetOrientation;

                    //if (!entity.isAlwaysActive &&
                    //    (entity.position - targetPosition).LengthSquared() > Toolbox.bigEpsilon ||
                    //    angularMotor.error.LengthSquared() > Toolbox.bigEpsilon))
                    //    entity.isActive = true;
                }
                else
                {
                    LinearMotor.IsActive = false;
                    AngularMotor.IsActive = false;

                    entity.LinearVelocity = getLinearVelocity(entity.Position, TargetPosition, dt);
                    entity.AngularVelocity = getAngularVelocity(entity.Orientation, TargetOrientation, dt);
                }
            }
        }
        
        /// <summary>
        /// Adds the motors to the space.  Called automatically.
        /// </summary>
        /// <param name="newSpace">Space the mover is being added to.</param>
        public override void OnAdditionToSpace(ISpace newSpace) 
        {
            newSpace.Add(LinearMotor);
           
            newSpace.Add(AngularMotor);
        }
        
        /// <summary>
        /// Removes the motors from the space.  Called automatically.
        /// </summary>
        public override void OnRemovalFromSpace(ISpace oldSpace)
        {
            oldSpace.Remove(LinearMotor);
            
            oldSpace.Remove(AngularMotor);
        }


        /// <summary>
        /// Gets the angular velocity necessary to change an entity's orientation from
        /// the starting quaternion to the ending quaternion over time dt.
        /// </summary>
        /// <param name="start">Initial orientation.</param>
        /// <param name="end">Final orientation.</param>
        /// <param name="dt">Time over which the angular velocity is to be applied.</param>
        /// <returns>Angular velocity to reach the goal in time.</returns>
        public static Vector3 getAngularVelocity(Quaternion start, Quaternion end, float dt)
        {

            //Compute the relative orientation R' between R and the target relative orientation.
            Quaternion errorOrientation;
            Quaternion.Conjugate(ref start, out errorOrientation);
            Quaternion.Multiply(ref errorOrientation, ref end, out errorOrientation);

            Vector3 axis;
            float angle;
            //Turn this into an axis-angle representation.
            Toolbox.GetAxisAngleFromQuaternion(ref errorOrientation, out axis, out angle);
            Vector3.Multiply(ref axis, angle / dt, out axis);
            return axis;
        }

        /// <summary>
        /// Gets the angular velocity necessary to change an entity's orientation from
        /// the starting quaternion to the ending quaternion over time dt.
        /// </summary>
        /// <param name="start">Initial position.</param>
        /// <param name="end">Final position.</param>
        /// <param name="dt">Time over which the angular velocity is to be applied.</param>
        /// <returns>Angular velocity to reach the goal in time.</returns>
        public static Vector3 getLinearVelocity(Vector3 start, Vector3 end, float dt)
        {
            Vector3 offset;
            Vector3.Subtract(ref end, ref start, out offset);
            Vector3.Divide(ref offset, dt, out offset);
            return offset;
        }

        public void Release()
        {
            entity = null;
            IsUpdating = false;
        }
    }
}
